#include <utility>

#include <ros/ros.h>

#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/image_encodings.h>

#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>

#include <image_geometry/pinhole_camera_model.h>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/ros/conversions.h>

#include <pcl/ModelCoefficients.h>
#include <pcl/features/normal_3d.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/passthrough.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>

//#include <pcl_visualization/cloud_viewer.h>

#include <cv_bridge/cv_bridge.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

#define max(a,b) (((a) > (b)) ? (a) : (b))
#define min(a,b) (((a) < (b)) ? (a) : (b))

using namespace ros;
using namespace sensor_msgs;
using namespace message_filters;
using namespace pcl;


void callback(const sensor_msgs::PointCloud2::ConstPtr &cloud_msg,
	      const sensor_msgs::Image::ConstPtr &image_msg,
	      const sensor_msgs::CameraInfo::ConstPtr &camera_info);
void hough_callback(const sensor_msgs::Image::ConstPtr &image_msg);
void do_segmentation(const PointCloud<PointXYZ>::Ptr &cloud, 
		     const double min_x, const double max_x,
		     const double min_y, const double max_y,
		     const double min_z, const double max_z,
		     double &a, double &b, double &c, double &d);
		     //		     cv::Point3d &knob3d);


void callback(const sensor_msgs::PointCloud2::ConstPtr &cloud_msg,
	      const sensor_msgs::Image::ConstPtr &image_msg,
	      const sensor_msgs::CameraInfo::ConstPtr &camera_info) {
  // Convert the image to an OpenCV image
  cv_bridge::CvImagePtr cv_ptr;
  try {
    cv_ptr = cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::BGR8);
  } catch (cv_bridge::Exception& e) {
    ROS_ERROR("cv_bridge exception: %s", e.what());
    return;
  }
  
  // Pre-defined region for knob.
  const int x1 = 430;
  const int y1 = 355;
  const int x2 = x1 + 70;
  const int y2 = y1 + 70;
  cv::rectangle(cv_ptr->image, cv::Point(x1, y1), cv::Point(x2, y2), CV_RGB(0, 255, 0));
   
  // Convert from ROS message to PCL type
  ROS_INFO("Doing point cloud conversion.");
  pcl::PointCloud<PointXYZ>::Ptr cloud (new pcl::PointCloud<PointXYZ>);
  fromROSMsg(*cloud_msg, *cloud);

  // Convert from image space to world space for winnowing out the point cloud.
  image_geometry::PinholeCameraModel camera_model;
  camera_model.fromCameraInfo(camera_info);
  cv::Point3d ul_ray = camera_model.projectPixelTo3dRay(cv::Point2d(x1, y1));
  cv::Point3d ur_ray = camera_model.projectPixelTo3dRay(cv::Point2d(x2, y1));
  cv::Point3d ll_ray = camera_model.projectPixelTo3dRay(cv::Point2d(x1, y2));
  cv::Point3d lr_ray = camera_model.projectPixelTo3dRay(cv::Point2d(x2, y2));

  // Get the distance ranges
  float min_z = 1000000.0;
  float max_z = -1.0;
  for(int u = x1; u <= x2; ++u)
    for(int v = y1; v <= y2; ++v) {
      min_z = min(min_z, cloud->at(u, v).z);
      max_z = max(max_z, cloud->at(u, v).z);
    }

  // Make an assumtion that there's nothing in front of the knob, and
  // that the knob doesn't stick out more than 10cm.
  max_z = min(max_z, min_z + 0.1f);

  // Figure out the x and y limits at the most distant point
  const double min_x = min(ul_ray.x, ll_ray.x) * min_z;
  const double max_x = max(ur_ray.x, lr_ray.x) * min_z;
  const double min_y = min(ul_ray.y, ur_ray.y) * min_z;
  const double max_y = min(ll_ray.y, lr_ray.y) * min_z;

  ROS_INFO("Doing segmentation.");
  //cv::Point3d knob3d;
  //  do_segmentation(cloud, min_x, max_x, min_y, max_y, min_z, max_z, knob3d);
  //
  //cv::Point knob = camera_model.project3dToPixel(knob3d);
  //cv::circle(cv_ptr->image, knob, 10, CV_RGB(255, 0, 0));
  //
  //cv::imshow("knobs", cv_ptr->image);

  //ROS_INFO("Waiting for keypress.");
  //cvWaitKey(0);

  double a, b, c, d;
  do_segmentation(cloud, min_x, max_x, min_y, max_y, min_z, max_z, a, b, c, d);

  std::cerr << "Plane: (" << a << ", " << b << ", " << c << ", " << d << ")" << std::endl;

  // Ray from center of selected area
  cv::Point3d ray = camera_model.projectPixelTo3dRay(cv::Point2d((x1 + x2) / 2.0, (y1 + y2) / 2.0));

  // Intersection of plane and line (assuming that everyone is in the same frame)
  double t = d / (a * ray.x + b * ray.y + c * ray.z);
  std::cerr << "t: " << t << ", point: (" << ray.x * t << ", " << ray.y * t << ", " << ray.z * t << ")" << std::endl;

  // Point on the end of the knob
  double x = ray.x * t;
  double y = ray.y * t;
  double z = ray.z * t;
  std::cerr << "Knob point: (" << x << ", " << y << ", " << z << ")" << std::endl;
}


void hough_callback(const sensor_msgs::Image::ConstPtr &image_msg) {
  cv_bridge::CvImagePtr image = cv_bridge::toCvCopy(image_msg, "mono8");

  // Do some processing to make the image cleaner
  cv::blur(image->image, image->image, cv::Size(3, 3));

  std::vector<cv::Vec3f> circles;
  cv::HoughCircles(image->image, circles, CV_HOUGH_GRADIENT, 1.0, 10.0, 100.0, 50.0, 10.0, 100.0);

  std::cerr << circles.size() << " circles found" << std::endl;
  for(std::vector<cv::Vec3f>::const_iterator it = circles.begin(); it != circles.end(); ++it) {
    std::cerr << "(" << (*it)[0] << ", " << (*it)[1] << "), radius " << (*it)[2] << std::endl;
 
    cv::circle(image->image, cv::Point2d((*it)[0], (*it)[1]), (*it)[2], CV_RGB(255, 255, 255));
  }
  
  cv::imshow("knobs", image->image);
  cvWaitKey(0);
}


void do_segmentation(const PointCloud<PointXYZ>::Ptr &cloud,
		     const double low_x, const double high_x,
		     const double low_y, const double high_y,
		     const double low_z, const double high_z,
		     double &a, double &b, double &c, double &d) {

		     //		     cv::Point3d &knob3d) {
  // All the objects needed
  pcl::PassThrough<PointXYZ> pass;
  pcl::NormalEstimation<PointXYZ, pcl::Normal> ne;
  pcl::SACSegmentationFromNormals<PointXYZ, pcl::Normal> seg;
  pcl::ExtractIndices<PointXYZ> extract;
  pcl::ExtractIndices<pcl::Normal> extract_normals;
  pcl::KdTreeFLANN<PointXYZ>::Ptr tree (new pcl::KdTreeFLANN<PointXYZ> ());
  
  // Datasets
  pcl::PointCloud<PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<PointXYZ>);
  pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
  pcl::ModelCoefficients::Ptr coefficients_plane (new pcl::ModelCoefficients), coefficients_cylinder (new pcl::ModelCoefficients);
  pcl::PointIndices::Ptr inliers_plane (new pcl::PointIndices), inliers_cylinder (new pcl::PointIndices);

  std::cerr << "x: (" << low_x << ", " << high_x << ")" << std::endl;
  std::cerr << "y: (" << low_y << ", " << high_y << ")" << std::endl;
  std::cerr << "z: (" << low_z << ", " << high_z << ")" << std::endl;


  // Hack
  pcl::PointCloud<PointXYZ>::Ptr cloud_filtered1 (new pcl::PointCloud<PointXYZ>);
  pcl::PointCloud<PointXYZ>::Ptr cloud_filtered2 (new pcl::PointCloud<PointXYZ>);

  // Build a passthrough filter to remove spurious NaNs
  pass.setInputCloud (cloud);
  pass.setFilterFieldName ("z");
  pass.setFilterLimits (low_z, high_z);
  pass.filter (*cloud_filtered1);
  std::cerr << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl;
  
  pass.setInputCloud (cloud_filtered1);
  pass.setFilterFieldName ("x");
  pass.setFilterLimits (low_x, high_x);
  pass.filter (*cloud_filtered2);

  pass.setInputCloud (cloud_filtered2);
  pass.setFilterFieldName ("y");
  pass.setFilterLimits (low_y, high_y);
  pass.filter (*cloud_filtered);


  // Estimate point normals
  ne.setSearchMethod (tree);
  ne.setInputCloud (cloud_filtered);
  ne.setKSearch (50);
  ne.compute (*cloud_normals);
#if 1
  // Create the segmentation object for the planar model and set all the parameters
  seg.setOptimizeCoefficients (true);
  seg.setModelType (pcl::SACMODEL_NORMAL_PLANE);
  seg.setNormalDistanceWeight (0.1);
  seg.setMethodType (pcl::SAC_RANSAC);
  seg.setMaxIterations (100);
  seg.setDistanceThreshold (0.05);
  seg.setInputCloud (cloud_filtered);
  seg.setInputNormals (cloud_normals);

  // Obtain the plane inliers and coefficients
  seg.segment (*inliers_plane, *coefficients_plane);
  std::cerr << "Plane coefficients: " << *coefficients_plane << std::endl;
  
  // Extract the planar inliers from the input cloud
  extract.setInputCloud (cloud_filtered);
  extract.setIndices (inliers_plane);
  extract.setNegative (false);
  
  // Write the planar inliers to disk
  pcl::PointCloud<PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<PointXYZ> ());
  extract.filter (*cloud_plane);
  std::cerr << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl;

  // Remove the planar inliers, extract the rest
  extract.setNegative (true);
  extract.filter (*cloud_filtered);
  extract_normals.setNegative (true);
  extract_normals.setInputCloud (cloud_normals);
  extract_normals.setIndices (inliers_plane);
  extract_normals.filter (*cloud_normals);

  a = coefficients_plane->values[0];
  b = coefficients_plane->values[1];
  c = coefficients_plane->values[2];
  d = coefficients_plane->values[3];
#endif

#if 0
  // Create the segmentation object for cylinder segmentation and set all the parameters
  seg.setOptimizeCoefficients (true);
  seg.setModelType (pcl::SACMODEL_CYLINDER);
  seg.setMethodType (pcl::SAC_RANSAC);
  seg.setNormalDistanceWeight (0.1);
  seg.setMaxIterations (10000);
  seg.setDistanceThreshold (0.005);
  seg.setRadiusLimits (0, 0.03);
  seg.setInputCloud (cloud_filtered);
  seg.setInputNormals (cloud_normals);
  
  // Obtain the cylinder inliers and coefficients
  seg.segment (*inliers_cylinder, *coefficients_cylinder);
  std::cerr << "Cylinder coefficients: " << *coefficients_cylinder << std::endl;
  
  // Write the cylinder inliers to disk
  extract.setInputCloud (cloud_filtered);
  extract.setIndices (inliers_cylinder);
  extract.setNegative (false);
  pcl::PointCloud<PointXYZ>::Ptr cloud_cylinder (new pcl::PointCloud<PointXYZ> ());
  extract.filter (*cloud_cylinder);
  std::cerr << "PointCloud representing the cylindrical component: " << cloud_cylinder->points.size () << " data points." << std::endl;
#endif

#if 0
  static pcl_visualization::CloudViewer viewer("Simple Cloud Viewer");
  viewer.showCloud(*cloud_filtered);
  //  while (!viewer.wasStopped());
#endif

#if 0
  knob3d.x = coefficients_cylinder->values[0];
  knob3d.y = coefficients_cylinder->values[1];
  knob3d.z = coefficients_cylinder->values[2];
#endif
}


int main(int argc, char **argv) {
  init(argc, argv, "knob_finder");

  NodeHandle node;

  // Synchronized subscriptions
  message_filters::Subscriber<PointCloud2> cloud_sub(node, "/kinect/rgb/points2", 10);
  message_filters::Subscriber<Image> image_sub(node, "/kinect/rgb/image_rect_color", 10);
  message_filters::Subscriber<CameraInfo> info_sub(node, "/kinect/rgb/camera_info", 10);
  TimeSynchronizer<PointCloud2, Image, CameraInfo> sync(cloud_sub, image_sub, info_sub, 10);
  sync.registerCallback(boost::bind(&callback, _1, _2, _3));

  //  ros::Subscriber hough_sub = node.subscribe("/narrow_stereo/right/image_mono", 10, hough_callback);
  ros::Subscriber hough_sub = node.subscribe("/r_forearm_cam/image_mono", 10, hough_callback);

  spin();

  return 0;
}

